/*
当使用仿真环境时，KM_VESC_current退化为KM_base
*/

# pragma once

# include "cpp_tools.h"
# include "udpSettings.h"
# include "dataStruct.h"
# include "KM_base.h"

# ifdef OUTPUT_REAL
# include "VESC_UART.h"
class KM_VESC_current : public chassis_controller
{
private:
    float current_max = 100; // 最大电流 A
    double GeerRatio = 48./15. * 43./10.; // 减速齿比(大于1) (-)
    double wheelRadius = 0.047; // 车轮半径 m
    double Kt = 60/(2*M_PI*2000); //转矩常数 Nm/A

    int drivingMode = 1; // 1: 前进, -1: 后退

    std::string VESC_device = "/dev/ttyACM0";
    int VESC_bauds = 115200;

    VescUart m_vesc;

    cpp_tools::udpVecSender<double> steeringSender;
    # define steeringSendIP "127.0.0.1"
    # define steeringSendPort 7303

    cpp_tools::mtDataManager<controlOutPut> output;
    
    const double steering_update = 100; // 转向pwm输出量的更新频率
    const double drive_update = 100; // 驱动pwm输出量的更新频率

    const double pi = M_PI;

    cpp_tools::threadPool m_threadPool;
    
    bool isRunning = true;

protected: 
    void outPutDrive(double drive)
    {
        float current_des = static_cast<float>(cpp_tools::saturation(drive, -1, 1)*current_max);

        if (drivingMode == 1)
        {
            if (current_des > 0)
            {
                m_vesc.setCurrent(current_des);
            }
            else
            {
                m_vesc.setBrakeCurrent(current_des);
            }
        }
        else if (drivingMode == -1)
        {
            current_des *= -1;

            if (current_des > 0)
            {
                m_vesc.setBrakeCurrent(current_des);
            }
            else
            {
                m_vesc.setCurrent(current_des);
            }
        }
    }

    void outPutSteering(double steering)
    {
        steeringSender.send(Eigen::Vector<double, 1>(steering));
    }

public:
    /// @brief 根据标定将期望的纵向力映射为油门量
    /// @param v 当前速度 m/s
    /// @param F_x_des 期望纵向力 N
    /// @return 油门控制量[-1,1]
    double forceMapping(double v, double F_x_des) const
    {
        // 阻力补偿
        double v_start = 1;
        double v_std = 3;
        double F_base = 2;
        double F_add = 13;
        if (abs(v) <= v_start)
        {
            F_x_des += F_add + F_base;
        }
        else if (abs(v) <= v_std)
        {
            F_x_des += (1 - (abs(v) - v_start)/(v_std - v_start))*F_add + F_base;
        }

        double current_des = F_x_des*wheelRadius/GeerRatio/Kt;

        return cpp_tools::saturation(current_des/current_max, -1, 1);
    };

    /// @brief 从期望前轮转角映射到RC控制的转向量
    /// @param delta_f_des 期望前轮转角, 单位 rad
    /// @return 返回范围为[-1,1]的转向行程 
    double steeringMapping(const double delta_f_des) const override
    {    
        double u;

        const double delta_f_max = 0.4;
        u = delta_f_des/delta_f_max;
    
        return cpp_tools::saturation(u,-1,1);
    };

    /// @brief 模式设置：左前进，右倒车
    /// @param D_pad_lr 
    void setMode(int D_pad_lr)
    {
        if (D_pad_lr == -1)
        {
            drivingMode = -1;

            std::cout << "设置为倒车模式" << std::endl;
        }
        else if (D_pad_lr == 1)
        {
            drivingMode = 1;

            std::cout << "设置为前进模式" << std::endl;
        }
    };
public:
    KM_VESC_current()
        : chassis_controller(), steeringSender(steeringSendIP, steeringSendPort, 1)
    {
        # ifdef OUTPUT_REAL
        cpp_tools::XmlParameterManager m_xml("device", host_path + settings_path + "/device/settings.xml");
        bool writeXML = false;
        if (!m_xml.getParameter("VESC", "fileName", VESC_device))
        {
            std::cout << "VESC设备路径使用使用默认值: " << VESC_device << std::endl;
            m_xml.setParameter("VESC", "fileName", VESC_device);

            writeXML = true;
        }
        if (!m_xml.getParameter("VESC", "bauds", VESC_bauds))
        {
            std::cout << "VESC波特率使用使用默认值: " << VESC_bauds << std::endl;
            m_xml.setParameter("VESC", "bauds", VESC_bauds);

            writeXML = true;
        }
        if (writeXML)
        {
            m_xml.saveToFile(host_path + settings_path + "/device/settings.xml");
        }

        m_vesc.setDebugStream(&std::cout);
        
        if (!m_vesc.connect(VESC_device, VESC_bauds))
        {
            std::cerr << "无法连接到VESC: " << VESC_device << std::endl;
        }

        m_vesc.setDebugStream(nullptr);
        # endif

        std::cout << "KM current start" << std::endl;

        // ==========================循环输出转角============================
        std::thread t_steering([&]() -> void {
            // 记录
            size_t steering_files = 0;
            cpp_tools::csvRecorder steeringRecorder(host_path + data_path + "/control/steering.csv");
            
            double steering_last = -9999;
            while (isRunning)
            {
                auto t = std::chrono::steady_clock::now();

                double steering = output.read().access()->steering;

                // 输出
                outPutSteering(steering);

                if (steering != steering_last)
                {
                    steering_last = steering;

                    // 保存
                    Eigen::VectorXd data(2);
                    data << cpp_tools::getCurrentTimeAsDouble(),steering;
                    steeringRecorder.record(data);
                }

                std::this_thread::sleep_until(t + std::chrono::microseconds(std::max((int)(1e6/steering_update),1)));
            }
        });
        m_threadPool.push_back(t_steering);

        // ==========================循环输出驱动力============================
        std::thread t_drive([&]() -> void {
            // 记录
            size_t drive_files = 0;
            cpp_tools::csvRecorder driveRecorder(host_path + data_path + "/control/drive.csv");
            
            double drive_last = -9999;
            while (isRunning)
            {
                auto t = std::chrono::steady_clock::now();

                double drive = output.read().access()->drive;

                // 输出
                outPutDrive(drive);

                if (drive != drive_last)
                {
                    drive_last = drive;

                    // 保存
                    Eigen::VectorXd data(2);
                    data << cpp_tools::getCurrentTimeAsDouble(),drive;
                    driveRecorder.record(data);
                }
                
                std::this_thread::sleep_until(t + std::chrono::microseconds(std::max((int)(1e6/drive_update),1)));
            }
        });
        m_threadPool.push_back(t_drive);
    };

    ~KM_VESC_current()
    {
        shutdown();

        std::cout << "output controller exit." << std::endl;
    };

    void shutdown() override
    {
        isRunning = false;

        m_threadPool.joinAll();
    }

    /// @brief 设置输出值
    /// @param steering 转向 [-1, 1]
    /// @param drive 驱动 [-1, 1]
    void setOutPut(double steering, double drive) override
    {
        output.write().access()->steering = steering;
        output.write().access()->drive = drive;
    }

    /// @brief 获取实际的输出值
    /// @return steering,drive
    controlOutPut getOutPut() override
    {
        return *output.read().access();
    }

    KM_VESC_current(const KM_VESC_current&) = delete;
};
# else
class KM_VESC_current : public KM_base
{
public:
    KM_VESC_current()
        : KM_base()
    {

    }
};
# endif
